#include "mpu6050_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "ti_msp_dl_config.h"  // 根据实际硬件驱动调整


#include "stdio.h"
#include "pid.h"
// #include "E:\CCSproject\DAC\rtostest\targetConfigs\mpu_6050\mpu6050.h"
#include "targetConfigs/mpu6050/mpu6050.h"
/* 私有函数和变量（仅本文件可见） */




/* 任务主函数 */
void Mpu6050Task(void *pvParameters) //只需要yaw轴
{
    // MPU6050_t x;
    // MPU6050_Init();
    // uint8_t ret = 1;
    // mpu6050_init();

    // float pitch=0,roll=0,yaw=0;   //欧拉角
    // // printf("start\r\n");
    // while( mpu_dmp_init() )
    // {
    //     // printf("dmp error\r\n");
    //     delay_ms(200);
    // }
    // printf("Initialization Data Succeed \r\n");
    /* 任务主循环 */
    for (;;) {
        // if( mpu_dmp_get_data(&pitch,&roll,&yaw) == 0 )
        // {
        //     // printf("\r\npitch =%d\r\n", (int)pitch);
        //     // printf("\r\nroll =%d\r\n", (int)roll);
        //     // printf("\r\nyaw =%d\r\n", (int)yaw);
        //     pid_angle.actual_val = yaw; 
        // }
        // MPU6050_Read_Gyro(&x);
        // printf("yaw:%lf\r\n",x.Gz);
        // Read_Quad();
        // printf("nihazo");
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    
    /* 理论上不会执行到这里 */
    vTaskDelete(NULL);
}
